/*=================================================================
 * theosResection.cpp : Single photo resection program for THEOS imagery
 *
 * Dr.Kanok Weerawong
 * 23/04/2010
 *=================================================================
 */

#include "stdafx.h"
#include <stdlib.h>
#include <math.h>
#include "theosResection.h"
using namespace std;
#define N_PAR 18

int _tmain(int argc, _TCHAR* argv[])
{

// 1. Read data and compute initial approximations for imaging parameters
	// Read parameters from DIMAP file 
	// Read image and ground coordinates from coordinate file


	// Prepare for reading coordinate file
	// Output: image and ground coordinates
	int nPoints, mImages, totalImagePoints;
	wstring coordinateFilename = L"../inputdata.txt";
	wstring outputFilename = L"../outputdata.txt";

	list <wstring> dimapFilenameList;
	GroundPointStruct *groundPoints = readCoordinates(coordinateFilename, dimapFilenameList,
										nPoints, mImages, totalImagePoints);
	
	// Prepare for reading DIMAP
	// Output: imaging parameters
	double **imagePar0 = new double*[mImages];
	DimapDataStruct *dimapFiles = new DimapDataStruct[mImages];

	// Read DIMAP files and compute initial approximations
	list<wstring>::iterator k;
	int imageIndex = 0;
	for (k=dimapFilenameList.begin(); k != dimapFilenameList.end(); ++k){
		dimapFiles[imageIndex] = readDimap(*k);
		imagePar0[imageIndex] = getApproximations(dimapFiles[imageIndex]);
		imageIndex++;
	}

	// Set model parameters
// Need fixed
// For now, assuming that the user set a flag to each parameter
// flag = 0 => fixed
// flag = 1 => adjustable
	int **parFlag = new int*[mImages];
	for (int i=0 ; i<mImages ; i++){
		parFlag[i] = new int[N_PAR];
		// Finally, this has to be done by the user.
		parFlag[i][0]  = 1;// a0
		parFlag[i][1]  = 0;// a1
		parFlag[i][2]  = 0;// a2
		parFlag[i][3]  = 1;// b0
		parFlag[i][4]  = 0;// b1
		parFlag[i][5]  = 0;// b2
		parFlag[i][6]  = 1;// c0
		parFlag[i][7]  = 0;// c1
		parFlag[i][8]  = 0;// c2
		parFlag[i][9]  = 1;// d0
		parFlag[i][10] = 1;// d1
		parFlag[i][11] = 0;// d2
		parFlag[i][12] = 1;// e0
		parFlag[i][13] = 1;// e1
		parFlag[i][14] = 0;// e2
		parFlag[i][15] = 1;// f0
		parFlag[i][16] = 1;// f1
		parFlag[i][17] = 0;// f2
	}

	int nAdjustedPar = 0;
	double **Delta0 = new double*[mImages];
	double **sigmaImagePar0 = new double*[mImages];
	for (int i=0 ; i<mImages ; i++){
		Delta0[i] = new double[N_PAR];
		for (int j=0 ; j<N_PAR ; j++){
			Delta0[i][j] = 0;
		}
		sigmaImagePar0[i] = new double[N_PAR];
		for (int k=0 ; k<9 ; k++){
			// a0 - c2 are related to coordinates
			if (parFlag[i][k] == 0){
				sigmaImagePar0[i][k]  = 0.001;// fixed
			} else {
				sigmaImagePar0[i][k]  = 100;// adjustable
				nAdjustedPar++;
			}// end if
			// d0 - f2 are related to angles
			if (parFlag[i][9+k] == 0){
				sigmaImagePar0[i][9+k]  = 0.001;// fixed
			} else {
				sigmaImagePar0[i][9+k]  = 100;// adjustable
				nAdjustedPar++;
			}// end if
		}
	}

// 2. Do adjustment
	// Set up parameters before calling adjustment()
	double **xyLos = new double*[mImages];
	double *refLine = new double[mImages];
	double *linePeriod = new double[mImages];
	double *centralRow = new double[mImages];
	TimeStruct *refTime = new TimeStruct[mImages];
	for (int i=0 ; i<mImages ; i++){
		xyLos[i] = new double[8];
		for (int j=0 ; j<8 ; j++){
			xyLos[i][j] = dimapFiles[i].xyLos[j];
		}
		refTime[i] = dimapFiles[i].refTime;
		refLine[i] = dimapFiles[i].refLine;
		linePeriod[i] = dimapFiles[i].linePeriod;
		centralRow[i] = dimapFiles[i].nRows/2;
	}

	// Open file for output
	ofstream outputfp(outputFilename.c_str());
	if(!outputfp) {
		cout << "Cannot open output file.\n";
	return 1;
	}
	outputfp << "Coordinate file: " << (wstring2String(coordinateFilename)).c_str() << endl;
	for (k=dimapFilenameList.begin(); k != dimapFilenameList.end(); ++k){
		outputfp << "DIMAP file: " << (wstring2String(*k)).c_str() << endl;
	}
	// Output parameters setting
	outputfp << "Parameters setting: 0 = fixed, 1 = adjustable" << endl;
	outputfp << "a0 => " << parFlag[0][0] << ",  a1 => " << parFlag[0][1];
	outputfp << ",  a2 => " << parFlag[0][2] << endl;
	outputfp << "b0 => " << parFlag[0][3] << ",  b1 => " << parFlag[0][4];
	outputfp << ",  b2 => " << parFlag[0][5] << endl;
	outputfp << "c0 => " << parFlag[0][6] << ",  c1 => " << parFlag[0][7];
	outputfp << ",  c2 => " << parFlag[0][8] << endl;
	outputfp << "d0 => " << parFlag[0][9] << ",  d1 => " << parFlag[0][10];
	outputfp << ",  d2 => " << parFlag[0][11] << endl;
	outputfp << "e0 => " << parFlag[0][12] << ",  e1 => " << parFlag[0][13];
	outputfp << ",  e2 => " << parFlag[0][14] << endl;
	outputfp << "f0 => " << parFlag[0][15] << ",  f1 => " << parFlag[0][16];
	outputfp << ",  f2 => " << parFlag[0][17] << endl;
	outputfp << "number of adjusted parameters = " << nAdjustedPar << endl;
	// Output ground coordinate values
	outputfp << endl << "Ground point coordinates in geocentric XYZ" << endl;
	outputfp.precision(3);
	outputfp.setf(ios::fixed,ios::floatfield); 
	for (int j=0 ; j<nPoints ; j++){
		outputfp << "Point # " ;
		outputfp << setw(5) << (groundPoints[j].id).c_str();
		outputfp << ",    X ="  << setw(14) << groundPoints[j].X;
		outputfp << ",  Y =" << setw(14) << groundPoints[j].Y;
		outputfp << ",  Z =" << setw(14) << groundPoints[j].Z << endl;
	}
	outputfp << endl << "===  Block adjustment  ===" << endl;
	// Call adjustment
	adjustment(centralRow, refTime, refLine, linePeriod, xyLos, nPoints, mImages, totalImagePoints,
				imagePar0, Delta0, sigmaImagePar0, nAdjustedPar, groundPoints, outputfp);
// 3. Compute statistics

	// Free memory
	delete [] groundPoints;
	delete [] Delta0;
	delete [] sigmaImagePar0;
	delete [] imagePar0;
	delete [] dimapFiles;
	delete [] xyLos;
	delete [] refLine;
	delete [] linePeriod;
	outputfp.close();
	return 0;
}

